#include <QtGui>
#include <QtOpenGL>
#include <QDebug>

#include <math.h>

#include "glwidget.h"

#include "core.h"
#include "robot.h"
#include "vector3D.h"
#include "coordinate3D.h"

GLWidget::GLWidget(Robot *rbt, QWidget *parent)
     : QGLWidget(parent)
{
	objects = 0;
	cylinderRadius = 0.25;

        xRot = 0;
        yRot = 0;
        zRot = 0;
        scale = 1.0;
        HorizontalMove = 0;
	VerticalMove = 0;
//         reset = 0;

	robot = rbt;
}

GLWidget::~GLWidget()
 {
     makeCurrent();
     glDeleteLists(objects, 3);
 }

 QSize GLWidget::minimumSizeHint() const
 {
     return QSize(50, 50);
 }

 QSize GLWidget::sizeHint() const
 {
     return QSize(400, 400);
 }


  void GLWidget::rotateBy(int xAngle, int yAngle, int zAngle)
 {
     xRot += xAngle;
     yRot += yAngle;
     zRot += zAngle;
     updateGL();
 }


void GLWidget::updateWidget()	//SLOT
{
    updateGL();
}

void GLWidget::robotLoaded()	//SLOT
{
	updateGL();
}

void GLWidget::initializeGL()
{
	glClearColor(0.8,0.8,0.8, 0.0);

	objects = glGenLists(3); //make lists for Cylinder, Prism, Gripper
	makeObjects();
	
	glShadeModel(GL_FLAT);
	
	glMatrixMode(GL_PROJECTION);
	
	glEnable(GL_DEPTH_TEST);
// 	glEnable(GL_CULL_FACE);
	glEnable(GL_LINE_SMOOTH);
}

void GLWidget::paintGL()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();

        glTranslated(0.0 + HorizontalMove,0.0 + VerticalMove, -8.0);
        glScalef(scale, scale, scale);

//         if(reset == 1){//reset the view
//             
//             reset = 0;
//         }

        glRotated(xRot, 0.0, 1.0, 0.0);
        glRotated(yRot, 1.0, 0.0, 0.0);

        drawGrid(12, 12.0f);

//        drawAxes(5.0f);
        drawRobotJoints();
        drawRobotLinks();

}

void GLWidget::resetView()
{
	HorizontalMove = 0;
        VerticalMove = 0;
        scale = 1;
        updateGL();
}

 void GLWidget::resizeGL(int width, int height)
 {
     int side = qMin(width, height);
     glViewport((width - side) / 2, (height - side) / 2, side, side);

     glMatrixMode(GL_PROJECTION);
     glLoadIdentity();
     glOrtho(-5.0, +5.0, -5.0, +5.0, -40.0, 40.0);
     glMatrixMode(GL_MODELVIEW);
 }

 void GLWidget::mousePressEvent(QMouseEvent *event)
 {
     lastPos = event->pos();
 }

void GLWidget::mouseMoveEvent(QMouseEvent *event)
{
     int dx = event->x() - lastPos.x();
     int dy = event->y() - lastPos.y();

     if (event->buttons() & Qt::LeftButton) {
         rotateBy(2 * dx, dy, 0);
     } else if (event->buttons() & Qt::RightButton) {
         //rotateBy(2 * dx, 0, 2 * dy);
         HorizontalMove += (float)dx/20;
         VerticalMove -= (float)dy/20;
     }
     lastPos = event->pos();
     updateGL();
 }

 void GLWidget::mouseReleaseEvent(QMouseEvent * /* event */)
 {
     emit clicked();
 }

 void GLWidget::wheelEvent(QWheelEvent *e)
 {
     e->delta() > 0 ? scale += scale*0.1f : scale -= scale*0.1f;
     updateGL();
 }

 void GLWidget::keyPressEvent(QKeyEvent *event){
    switch (event->key()) {
     case Qt::Key_Up:
         VerticalMove += 0.2f;
         break;
     case Qt::Key_Down:
         VerticalMove -= 0.2f;
         break;
     case Qt::Key_Left:
         HorizontalMove -= 0.2f;
         break;
     case Qt::Key_Right:
         HorizontalMove += 0.2f;
         break;
    default:
         GLWidget::keyPressEvent(event);
     }
    updateGL();
 }

void GLWidget::drawAxes(const float lenght)
{
    glBegin(GL_LINES); {
                glColor3f(0.0f, 1.0f, 1.0f);
                glVertex3f(0.0f, 0.0f, 0.0f);
                glVertex3f(lenght, 0.0f, 0.0f);

                glColor3f(0.0f, 1.0f, 0.0f);
                glVertex3f(0.0f, 0.0f, 0.0f);
                glVertex3f(0.0f, lenght, 0.0f);

                glColor3f(0.0f, 0.0f, 1.0f);
                glVertex3f(0.0f, 0.0f, 0.0f);
                glVertex3f(0.0f, 0.0f, lenght);
        } glEnd();
}

void GLWidget::drawGrid(const int nSquares, const float size)
{
        float xstart = -size/2.0f;
        float zstart = xstart;
        float incr = size/(float)nSquares;
        for(int x=0; x < nSquares; x++) {//draw bottom
                for(int y=0; y < nSquares; y++) {
                        glBegin(GL_LINES);
                        glColor3f(0.75, 0.7, 0.8);
                        if( x < nSquares-1 ) {
                                // Draw right
                                glVertex3f(xstart, zstart,-1 );
                                glVertex3f(xstart+incr, zstart,-1 );
                        }
                        if( y < nSquares-1 ) {
                                // Draw down
                                glVertex3f(xstart, zstart,-1 );
                                glVertex3f(xstart, zstart+incr,-1 );
                        }
                        glEnd();
                        zstart+=incr;
                }
                xstart+=incr;
                zstart = -size/2.0f;
        }
}

void GLWidget::drawRobotJoints()
{
        int type;
        float x_temp, y_temp, z_temp;
        bool ok;
        
        coord3D center1(0.0f,0.0f,0.0f);
        coord3D prev_center(-1.0f,-1.0f,-1.0f);
        coord3D center(0.0f,0.0f,0.0f);
        coord3D next_center(-1.0f,-1.0f,-1.0f);
        coord3D next_center2(-1.0f,-1.0f,-1.0f);
        //Analyze and modify centers of current joint and nearby joints
        for (int i = 0; i < robot->getJointCount(); i++){

            center.setValues(robot->jointList[i].getStaticCenter().x(),
                             robot->jointList[i].getStaticCenter().y(),
                             robot->jointList[i].getStaticCenter().z());
        //set previous center
            if (i > 0){
                prev_center.setValues(robot->jointList[i-1].getStaticCenter().x(),
                                      robot->jointList[i-1].getStaticCenter().y(),
                                      robot->jointList[i-1].getStaticCenter().z());
            }else {
                prev_center.setValues(-1.0f,-1.0f,-1.0f);
            }
        //set next center
            if (i < robot->getJointCount()-1){
                next_center.setValues(robot->jointList[i+1].getStaticCenter().x(),
                                      robot->jointList[i+1].getStaticCenter().y(),
                                      robot->jointList[i+1].getStaticCenter().z());
            }else {
                next_center.setValues(-1.0f,-1.0f,-1.0f);
            }
        //set next next center
            if (i < robot->getJointCount()-2){
                next_center2.setValues(robot->jointList[i+2].getStaticCenter().x(),
                                       robot->jointList[i+2].getStaticCenter().y(),
                                       robot->jointList[i+2].getStaticCenter().z());
            }else {
                next_center2.setValues(-1.0f,-1.0f,-1.0f);
            }
            if (center.isEqual(next_center) && center.isEqual(next_center2)){
                robot->jointList[i].setStaticCenter(coord3D(center.x()- robot->jointList[i].staticOrientationK().x(),
                                                    center.y()- robot->jointList[i].staticOrientationK().y(),
                                                    center.z()- robot->jointList[i].staticOrientationK().z()));

            }else if(center.isEqual(prev_center)){
                robot->jointList[i].setStaticCenter(coord3D(center.x()+ robot->jointList[i].staticOrientationK().x(),
                                                    center.y()+ robot->jointList[i].staticOrientationK().y(),
                                                    center.z()+ robot->jointList[i].staticOrientationK().z()));
            }
        }

        //draw stuff
        for (int i = 0; i < robot->getJointCount() ; i ++){

            center.setValues(robot->jointList[i].getStaticCenter().x(),
                             robot->jointList[i].getStaticCenter().y(),
                             robot->jointList[i].getStaticCenter().z());
            if (i < robot->getJointCount()-1){
                next_center.setValues(robot->jointList[i+1].getStaticCenter().x(),
                                      robot->jointList[i+1].getStaticCenter().y(),
                                      robot->jointList[i+1].getStaticCenter().z());
            }
        //draw joints------------------------------------------------------------
                glTranslated(center.x(),
                             center.y(),
                             center.z());//move to new center

            if (robot->jointList[i].getType() == REVOLUTE){//if revolute joint

                glRotated(robot->jointList[i].getRotation(&ok),
                                   robot->jointList[i].staticOrientationK().x(),
                                   robot->jointList[i].staticOrientationK().y(),
                                   robot->jointList[i].staticOrientationK().z());//rotate around staticOrientationK

                drawCylinder(center1, robot->jointList[i].staticOrientationK(), robot->jointList[i].axesDrawn());

            }else if (robot->jointList[i].getType() == PRISMATIC){ //if prismatic joint

                drawPrism(center1, robot->jointList[i].staticOrientationK(), robot->jointList[i].axesDrawn());
                glTranslated(robot->jointList[i].staticOrientationK().x()*(robot->jointList[i].getExtension(&ok)),
                             robot->jointList[i].staticOrientationK().y()*(robot->jointList[i].getExtension(&ok)),
                             robot->jointList[i].staticOrientationK().z()*(robot->jointList[i].getExtension(&ok)));//translate along orientation
            }else if (robot->jointList[i].getType() == GRIPPER){
                drawGripper(center1,
                            robot->jointList[i].staticOrientationK(),
                            robot->jointList[i].axesDrawn(),
                            robot->jointList[i].getGripperStatus(&ok));
            }
                glTranslated(-center.x(),//Revert the changes to the state machine
                             -center.y(),
                             -center.z());

            //Draw links------------------------------------------------------------
            if (i != robot->getJointCount()-1){//make sure that there is always a next joint
                glBegin(GL_LINES);
                //creating coordinates for the point at the end of the first segment of any link
                coord3D nextP(center.x()+robot->jointList[i].staticOrientationK().x(),
                           center.y()+robot->jointList[i].staticOrientationK().y(),
                           center.z()+robot->jointList[i].staticOrientationK().z());
                coord3D nextP2(next_center.x()+robot->jointList[i+1].staticOrientationK().x(),
                               next_center.y()+robot->jointList[i+1].staticOrientationK().y(),
                               next_center.z()+robot->jointList[i+1].staticOrientationK().z());
                //Link leaving upstream (first segment)
                if (robot->jointList[i].getType() == PRISMATIC){//different from other links because center changes
                    glVertex3f(center.x()- robot->jointList[i].staticOrientationK().x()*(robot->jointList[i].getExtension(&ok)),
                               center.y()- robot->jointList[i].staticOrientationK().y()*(robot->jointList[i].getExtension(&ok)),
                               center.z()- robot->jointList[i].staticOrientationK().z()*(robot->jointList[i].getExtension(&ok))); // origin of the line
                    glVertex3f(nextP.x(),nextP.y(),nextP.z());
                }else{
                    glVertex3f(center.x(),
                               center.y(),
                               center.z()); // origin of the line
                    glVertex3f(nextP.x(),nextP.y(),nextP.z());
                }

                //next segments need to be decided depending on location of the centers of both joints
                if (center.toVector(next_center).isEqual(
                        center.toVector(nextP))){
                    type = 0;//Joint's center is along the upstream axis
                }else if(nextP.toVector(next_center).dot(
                        center.toVector(nextP)) == 0){
                    type = 0;//Joint's center is at a 90 degree angle to the upstream axis
                }else if(nextP.toVector(center).dot(
                        next_center.toVector(nextP2)) == 0 ){
                    type = 1;//Joints' upstream axis are perpendicular to each other
                }else{
                    type = 2;//Joint is somewhere else
                }
                switch (type){
                    case 0:
                        glVertex3f(nextP.x(),nextP.y(),nextP.z());
                        glVertex3f(next_center.x(),
                                   next_center.y(),
                                   next_center.z());
                        break;
                    case 1:
                        //first create point that will connect nextP with next_center
                        if(floor((nextP.x()*robot->jointList[i].staticOrientationK().x())*100.0+0.5)/100 != 0){
                            x_temp = nextP.x();
                            y_temp = next_center.y();
                            z_temp = next_center.z();
                        }else if(floor((nextP.z()*robot->jointList[i].staticOrientationK().z())*100.0+0.5)/100 != 0){
                            x_temp = next_center.x();
                            y_temp = next_center.y();
                            z_temp = nextP.z();
                        }else{
                            x_temp = next_center.x();
                            y_temp = nextP.y();
                            z_temp = next_center.z();
                        }
                        //draw the lines
                        glVertex3f(nextP.x(),nextP.y(),nextP.z());
                        glVertex3f(x_temp,y_temp,z_temp);
                        glVertex3f(x_temp,y_temp,z_temp);
                        glVertex3f(next_center.x(),
                                   next_center.y(),
                                   next_center.z());
                        break;
                    case 2:
                        //line between nextP and midpoint between center i and center i+1 using i's Y coordinate
                        glVertex3f(nextP.x(),nextP.y(),nextP.z());
                        glVertex3f((nextP.x() + next_center.x())/2,
                                   nextP.y(),
                                   (nextP.z() + next_center.z())/2);
                        //line between midpoint using i's Y and midpoint using i+1's Y
                        glVertex3f((nextP.x()+ next_center.x())/2,
                                   nextP.y(),
                                   (nextP.z()+ next_center.z())/2);
                        glVertex3f((nextP.x()+ next_center.x())/2,
                                   next_center.y(),
                                   (nextP.z()+ next_center.z())/2);
                        //line between midpoint of i+1 and center of i+1
                        glVertex3f((nextP.x()+ next_center.x())/2,
                                   next_center.y(),
                                   (nextP.z()+ next_center.z())/2);
                        glVertex3f(next_center.x(),
                                   next_center.y(),
                                   next_center.z());
                        break;
                }
                glEnd( );
            }//end draw links
        }//end draw stuff
}

void GLWidget::drawRobotLinks()
{

}

void GLWidget::drawTube(coord3D start, coord3D end)
{
	

}

void GLWidget::makeObjects()
{
	makeCylinder(100);
	makePrism();
	makeGripper();
}

void GLWidget::makeCylinder(int cylinderSlices)
{
	//generate the generate 2D coords of vertices of the slices
	//these are later extruded into a cylinder using quads for the walls and triangles for lids
	GLfloat *slice_position;
	slice_position = (GLfloat*) calloc(2*(cylinderSlices+1), sizeof(GLfloat));
	
	const GLfloat Pi = 3.14159265358979323846;
	
	for (int i=0 ; i<cylinderSlices; i++)
	{
		//x, z stored one after another
		slice_position[i*2] = cos(((Pi*2)/cylinderSlices) * i)*cylinderRadius;
		slice_position[i*2+1] = sin(((Pi*2)/cylinderSlices) * i)*cylinderRadius;
		
	}
	slice_position[cylinderSlices*2] = slice_position[0];
	slice_position[cylinderSlices*2+1] = slice_position[1];
	
	glNewList(objects + CYLINDER, GL_COMPILE);
		glBegin(GL_QUADS);		
			glColor3f(0.0, 0.7, 0.0);
			for (int i=1; i< cylinderSlices; i++)
			{	
				//set colour (each quarter is different color)
				if (i < 25 || ((i < 75) && (i > 50)))
				{
					glColor3f(0.0, 0.7, 0.0);
				}
				else
				{
					glColor3f(0.0, 0.5, 0.0);
				}
				
				//wall
				glVertex3f(slice_position[i*2], 0.5, slice_position[i*2+1]);
				glVertex3f(slice_position[i*2], -0.5, slice_position[i*2+1]);
				glVertex3f(slice_position[(i+1)*2], -0.5, slice_position[(i+1)*2+1]);
				glVertex3f(slice_position[(i+1)*2], 0.5, slice_position[(i+1)*2+1]);	
			}
		glEnd();
		
		glBegin(GL_TRIANGLES);
			glColor3f(0.0, 0.7 ,0.0);
			for (int i = 0; i<cylinderSlices; i++)
			{
				//set colour (each quarter is different color)
				if (i < 25 || ((i < 75) && (i > 50)))
				{
					glColor3f(0.0, 0.7, 0.0);
				}
				else
				{
					glColor3f(0.0, 0.5, 0.0);
				}
				
				//top lid
				glVertex3f(0.0, 0.5, 0.0);
				glVertex3f(slice_position[i*2], 0.5, slice_position[i*2+1]);
				glVertex3f(slice_position[(i+1)*2], 0.5, slice_position[(i+1)*2+1]);
				
				//bottom lid
				glVertex3f(0.0, -0.5, 0.0);
				glVertex3f(slice_position[i*2], -0.5, slice_position[i*2+1]);
				glVertex3f(slice_position[(i+1)*2], -0.5, slice_position[(i+1)*2+1]);
			}	
		glEnd();
		
		glColor3f(0.0, 0.0, 0.0);
		glLineWidth(2.0);
		//draw top lid outline
		glBegin(GL_LINE_LOOP);
			for (int i =0 ; i<cylinderSlices; i++)
			{
				glVertex3f(slice_position[i*2], 0.5, slice_position[i*2+1]);
			}
		glEnd();
		
		//draw bottom lid outline
		glBegin(GL_LINE_LOOP);
			for (int i =0 ; i<cylinderSlices; i++)
			{
				glVertex3f(slice_position[i*2], -0.5, slice_position[i*2+1]);
			}
		glEnd();
	glEndList();
	free(slice_position);
}

void GLWidget::drawCylinder(coord3D center, vector3D orientation, bool axes)
{
        vector3D up(0.0f,1.0f,0.0f);
        glMatrixMode(GL_MODELVIEW);
        glPushMatrix();

        glRotated(up.angleBetween(orientation),
                  up.cross(orientation).x(),
                  up.cross(orientation).y(),
                  up.cross(orientation).z());
        glScaled(0.9f,0.9f,0.9f);

        if (axes) drawAxes(2.0f); //to draw Axes
	glCallList(objects + CYLINDER);

        glPopMatrix();
}

void GLWidget::makePrism()
{
	glNewList(objects + PRISM, GL_COMPILE);
		glBegin(GL_QUADS);
			glVertex3f(-0.25, 0.5, -0.25);
			glVertex3f(-0.25, 0.5, 0.25);
			glVertex3f(0.25, 0.5, 0.25);
			glVertex3f(0.25, 0.5, -0.25);
			
			glVertex3f(-0.25, -0.5, -0.25);
			glVertex3f(-0.25, -0.5, 0.25);
			glVertex3f(0.25, -0.5, 0.25);
			glVertex3f(0.25, -0.5, -0.25);
		glEnd();
		
		glBegin(GL_QUAD_STRIP);
			glVertex3f(-0.25, 0.5, -0.25);
			glVertex3f(-0.25, -0.5, -0.25);
			glVertex3f(-0.25, 0.5, 0.25);
			glVertex3f(-0.25, -0.5, 0.25);
			glVertex3f(0.25, 0.5, 0.25);
			glVertex3f(0.25, -0.5, 0.25);
			glVertex3f(0.25, 0.5, -0.25);
			glVertex3f(0.25, -0.5, -0.25);
			glVertex3f(-0.25, 0.5, -0.25);
			glVertex3f(-0.25,-0.5, -0.25);
		glEnd();
	glEndList();
}
 
void GLWidget::drawPrism(coord3D center, vector3D orientation, bool axes)
{
        vector3D up(0.0f,1.0f,0.0f);
        glPushMatrix();
	//move to the correct position and orientation (can't do this until i know center and orienation
	//for an arbitrary rotation for any joint
        glRotated(up.angleBetween(orientation),
                  up.cross(orientation).x(),
                  up.cross(orientation).y(),
                  up.cross(orientation).z());
        glScaled(0.9f,0.9f,0.9f);

        if (axes) drawAxes(2.0f); //to draw Axes
	//draw the prism
	glPushAttrib(GL_POLYGON_BIT);
	glColor3f(0.7,0.0,0.0);
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glCallList(objects+PRISM);
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glColor3f(0.0, 0.0, 0.0);
	glCallList(objects+PRISM);
 	glPopAttrib();
 	glPopMatrix();
}


void GLWidget::makeGripper()
{
	glNewList(objects + GRIPPER_OBJ, GL_COMPILE);
		glColor3f(0.1, 0.1, 0.7);
                glBegin(GL_QUADS);
                        //horizontal
                        glVertex3f(-0.15, 0.25, 0.1);//front
                        glVertex3f(-0.15, 0.1, 0.1);
                        glVertex3f(0.15, 0.1, 0.1);
                        glVertex3f(0.15, 0.25, 0.1);
                        glVertex3f(-0.15, 0.25, -0.1);//back
                        glVertex3f(-0.15, 0.1, -0.1);
                        glVertex3f(0.15, 0.1, -0.1);
                        glVertex3f(0.15, 0.25, -0.1);
                        glVertex3f(-0.15, 0.25, 0.1);//top
                        glVertex3f(0.15, 0.25, 0.1);
                        glVertex3f(0.15, 0.25, -0.1);
                        glVertex3f(-0.15, 0.25, -0.1);
                        glVertex3f(-0.15, 0.1, 0.1);//bottom
                        glVertex3f(0.15, 0.25, 0.1);
                        glVertex3f(0.15, 0.1, -0.1);
                        glVertex3f(-0.15, 0.1, -0.1);
                glEnd();
                    glPushMatrix();
//                        glTranslated(-0.10,0.0,0.0);
 //                      glRotated(15,0.0,0.0,1.0);
//                        glTranslated(0.20,0.0,0.0);
                glBegin(GL_QUADS);
                        //left finger
                        glVertex3f(-0.25, 0.25, 0.1);//front
			glVertex3f(-0.25, -0.25, 0.1);
			glVertex3f(-0.15, -0.25, 0.1);
			glVertex3f(-0.15, 0.25, 0.1);
                        glVertex3f(-0.25, 0.25, -0.1);//back
                        glVertex3f(-0.25, -0.25, -0.1);
                        glVertex3f(-0.15, -0.25, -0.1);
                        glVertex3f(-0.15, 0.25, -0.1);
                        glVertex3f(-0.25, 0.25, 0.1);//top
                        glVertex3f(-0.15, 0.25, 0.1);
                        glVertex3f(-0.15, 0.25, -0.1);
                        glVertex3f(-0.25, 0.25, -0.1);
                        glVertex3f(-0.25, -0.25, 0.1);//bottom
                        glVertex3f(-0.15, -0.25, 0.1);
                        glVertex3f(-0.15, -0.25, -0.1);
                        glVertex3f(-0.25, -0.25, -0.1);
                        glVertex3f(-0.25, 0.25, -0.1);//left side
                        glVertex3f(-0.25, -0.25, -0.1);
                        glVertex3f(-0.25, -0.25, 0.1);
                        glVertex3f(-0.25, 0.25, 0.1);
                        glVertex3f(-0.15, 0.25, 0.1);//right side
                        glVertex3f(-0.15, -0.25, 0.1);
                        glVertex3f(-0.15, -0.25, -0.1);
                        glVertex3f(-0.15, 0.25, -0.1);
                   glEnd();
                      glPopMatrix();
                      glPushMatrix();

//                       glRotated(-15,0.0,0.0,1.0);
//                        glTranslated(-0.20,0.0,0.0);
                   glBegin(GL_QUADS);
                        //right finger
                        glVertex3f(0.15, 0.25, 0.1);//front
                        glVertex3f(0.15, -0.25, 0.1);
                        glVertex3f(0.25, -0.25, 0.1);
                        glVertex3f(0.25, 0.25, 0.1);
                        glVertex3f(0.15, 0.25, -0.1);//back
                        glVertex3f(0.15, -0.25, -0.1);
                        glVertex3f(0.25, -0.25, -0.1);
                        glVertex3f(0.25, 0.25, -0.1);
                        glVertex3f(0.15, 0.25, 0.1);//top
                        glVertex3f(0.25, 0.25, 0.1);
                        glVertex3f(0.25, 0.25, -0.1);
                        glVertex3f(0.15, 0.25, -0.1);
                        glVertex3f(0.15, -0.25, -0.1);//bottom
                        glVertex3f(0.25, -0.25, -0.1);
                        glVertex3f(0.25, -0.25, 0.1);
                        glVertex3f(0.15, -0.25, 0.1);
                        glVertex3f(0.25, 0.25, -0.1);//left side
                        glVertex3f(0.15, -0.25, -0.1);
                        glVertex3f(0.15, -0.25, 0.1);
                        glVertex3f(0.25, 0.25, 0.1);
                        glVertex3f(0.25, 0.25, 0.1);//right side
                        glVertex3f(0.25, -0.25, 0.1);
                        glVertex3f(0.25, -0.25, -0.1);
                        glVertex3f(0.25, 0.25, -0.1);
                glEnd();
                  glPopMatrix();
	glEndList();
                glNewList(objects + GRIPPER_OBJ1, GL_COMPILE);
                glColor3f(0.1, 0.1, 0.7);
                glBegin(GL_QUADS);
                        //horizontal
                        glVertex3f(-0.15, 0.25, 0.1);//front
                        glVertex3f(-0.15, 0.1, 0.1);
                        glVertex3f(0.15, 0.1, 0.1);
                        glVertex3f(0.15, 0.25, 0.1);
                        glVertex3f(-0.15, 0.25, -0.1);//back
                        glVertex3f(-0.15, 0.1, -0.1);
                        glVertex3f(0.15, 0.1, -0.1);
                        glVertex3f(0.15, 0.25, -0.1);
                        glVertex3f(-0.15, 0.25, 0.1);//top
                        glVertex3f(0.15, 0.25, 0.1);
                        glVertex3f(0.15, 0.25, -0.1);
                        glVertex3f(-0.15, 0.25, -0.1);
                        glVertex3f(-0.15, 0.1, 0.1);//bottom
                        glVertex3f(0.15, 0.25, 0.1);
                        glVertex3f(0.15, 0.1, -0.1);
                        glVertex3f(-0.15, 0.1, -0.1);
                glEnd();
                    glPushMatrix();
                        glTranslated(-0.10,0.0,0.0);
                        glRotated(15,0.0,0.0,1.0);
                        glTranslated(0.20,0.0,0.0);
                glBegin(GL_QUADS);
                        //left finger
                        glVertex3f(-0.25, 0.25, 0.1);//front
                        glVertex3f(-0.25, -0.25, 0.1);
                        glVertex3f(-0.15, -0.25, 0.1);
                        glVertex3f(-0.15, 0.25, 0.1);
                        glVertex3f(-0.25, 0.25, -0.1);//back
                        glVertex3f(-0.25, -0.25, -0.1);
                        glVertex3f(-0.15, -0.25, -0.1);
                        glVertex3f(-0.15, 0.25, -0.1);
                        glVertex3f(-0.25, 0.25, 0.1);//top
                        glVertex3f(-0.15, 0.25, 0.1);
                        glVertex3f(-0.15, 0.25, -0.1);
                        glVertex3f(-0.25, 0.25, -0.1);
                        glVertex3f(-0.25, -0.25, 0.1);//bottom
                        glVertex3f(-0.15, -0.25, 0.1);
                        glVertex3f(-0.15, -0.25, -0.1);
                        glVertex3f(-0.25, -0.25, -0.1);
                        glVertex3f(-0.25, 0.25, -0.1);//left side
                        glVertex3f(-0.25, -0.25, -0.1);
                        glVertex3f(-0.25, -0.25, 0.1);
                        glVertex3f(-0.25, 0.25, 0.1);
                        glVertex3f(-0.15, 0.25, 0.1);//right side
                        glVertex3f(-0.15, -0.25, 0.1);
                        glVertex3f(-0.15, -0.25, -0.1);
                        glVertex3f(-0.15, 0.25, -0.1);
                   glEnd();
                        glPopMatrix();
                        glPushMatrix();

                        glRotated(-15,0.0,0.0,1.0);
                        glTranslated(-0.13,0.0,0.0);
                   glBegin(GL_QUADS);
                        //right finger
                        glVertex3f(0.15, 0.25, 0.1);//front
                        glVertex3f(0.15, -0.25, 0.1);
                        glVertex3f(0.25, -0.25, 0.1);
                        glVertex3f(0.25, 0.25, 0.1);
                        glVertex3f(0.15, 0.25, -0.1);//back
                        glVertex3f(0.15, -0.25, -0.1);
                        glVertex3f(0.25, -0.25, -0.1);
                        glVertex3f(0.25, 0.25, -0.1);
                        glVertex3f(0.15, 0.25, 0.1);//top
                        glVertex3f(0.25, 0.25, 0.1);
                        glVertex3f(0.25, 0.25, -0.1);
                        glVertex3f(0.15, 0.25, -0.1);
                        glVertex3f(0.15, -0.25, -0.1);//bottom
                        glVertex3f(0.25, -0.25, -0.1);
                        glVertex3f(0.25, -0.25, 0.1);
                        glVertex3f(0.15, -0.25, 0.1);
                        glVertex3f(0.25, 0.25, -0.1);//left side
                        glVertex3f(0.15, -0.25, -0.1);
                        glVertex3f(0.15, -0.25, 0.1);
                        glVertex3f(0.25, 0.25, 0.1);
                        glVertex3f(0.25, 0.25, 0.1);//right side
                        glVertex3f(0.25, -0.25, 0.1);
                        glVertex3f(0.25, -0.25, -0.1);
                        glVertex3f(0.25, 0.25, -0.1);
                glEnd();
                  glPopMatrix();
        glEndList();

}

void GLWidget::drawGripper(coord3D center, vector3D orientation, bool axes, bool clenched)
{
        vector3D up(0.0f,1.0f,0.0f);
	glPushMatrix();
            glRotated(up.angleBetween(orientation),
                    up.cross(orientation).x(),
                    up.cross(orientation).y(),
                    up.cross(orientation).z());

            glScaled(0.9f,0.9f,0.9f);
            if (axes) drawAxes(2.0f); //to draw Axes
            glScaled(1.0f,-1.0f,1.0f);//flip the gripper vertically

            if (!clenched){ glCallList(objects+GRIPPER_OBJ);
            }else{glCallList(objects+GRIPPER_OBJ1);}

	glPopMatrix();
}

void GLWidget::normalizeAngle(int *angle)
{
     while (*angle < 0)
         *angle += 360 * 16;
     while (*angle > 360 * 16)
         *angle -= 360 * 16;
}
void GLWidget::computeThirdPoint(float *point_X, float *point_Y, float x1, float x2, float y1, float y2)
{
    float dx, slope, midpoint_x, midpoint_y, height;

    dx = x1 - x2;
    midpoint_x = (x1+x2)/2;
    midpoint_y = (y1+y2)/2;
    height = midpoint_x;
    slope = -dx / (y1 - y2);
    *point_X = sqrt(sqrt((height*height - dx*dx)*(height*height - dx*dx)))/slope + midpoint_x;
    *point_Y = slope * (*point_X - midpoint_x) + midpoint_y;
}
